﻿/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 *
 * Project: 9dof: Razor 9DoF AHRS firmware and NETMF drivers
 * Description:  This is a collection of files that allows for easy
 * NETMf integration with the  * Sparkfun 9DoF Razor IMU.
 *
 * Author(s):
 * Chris Seto <chris@chrisseto.com>
 * Christian Völlinger <zerov83@googlemail.com>
 * 
 */

using System;
using System.IO.Ports;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using NineDofAhrs.Extensions;

namespace NineDofAhrs
{
	/// <summary>
	/// Class to parse serial output of Sparkfun 9DOF Razor AHRS
	/// Call ParseData() before you want to use new data!
	/// </summary>
	public class RazorAhrs : IAhrs
	{
		/// <summary>
		/// Serial port handle for the 9DoF
		/// </summary>
		private readonly SerialPort serialHandle;

		/// <summary>
		/// USed to reset the 9DoF
		/// </summary>
		private readonly Cpu.Pin dtr;

		/// <summary>
		/// 
		/// </summary>
		private readonly byte[] receiveBuffer = new byte[80];

		/// <summary>
		/// 
		/// </summary>
		private readonly char[] receivedChars = new char[80];

		/// <summary>
		/// 
		/// </summary>
		private readonly char[] parseBuffer = new char[80];

		/// <summary>
		/// 
		/// </summary>
		private int receiveIndex = 0;

		/// <summary>
		/// 
		/// </summary>
		private bool haveToParse = false;

		/// <summary>
		/// 
		/// </summary>
		private ParserState imuState = ParserState.FindPre;

		/// <summary>
		/// 
		/// </summary>
		private static readonly char[] pree = { '!', 'A', 'N', 'G', ':' };

		/// <summary>
		/// 
		/// </summary>
		private int preeIndex = 0;

		/// <summary>
		/// 
		/// </summary>
		private const string analogsPree = ",AN:";

		/// <summary>
		/// 
		/// </summary>
		private const string productSring = "Sparkfun 9DOF Razor AHRS";

		/// <summary>
		/// 
		/// </summary>
		public bool HasValidData = false;

		/// <summary>
		/// 
		/// </summary>
		public float Roll;

		/// <summary>
		/// 
		/// </summary>
		public float Pitch;

		/// <summary>
		/// 
		/// </summary>
		public float Yaw;

		/// <summary>
		/// AccX, AccY, AccZ, AnX, AnY, AnZ
		/// </summary>
		public int[] Offsets = new int[6];

		/// <summary>
		/// 
		/// </summary>
		public int AccX;

		/// <summary>
		/// 
		/// </summary>
		public int AccY;

		/// <summary>
		/// 
		/// </summary>
		public int AccZ;

		/// <summary>
		/// 
		/// </summary>
		public int AnX;

		/// <summary>
		/// 
		/// </summary>
		public int AnY;

		/// <summary>
		/// 
		/// </summary>
		public int AnZ;

		/// <summary>
		/// 
		/// </summary>
		public int MagX;

		/// <summary>
		/// 
		/// </summary>
		public int MagY;

		/// <summary>
		/// 
		/// </summary>
		public int MagZ;

		/// <summary>
		/// Are we going to parse analog outputs?
		/// </summary>
		private readonly bool ParseAnalogs = false;

		/// <summary>
		/// Parser phases
		/// </summary>
		private enum ParserState
		{
			Start,
			FindOffsets,
			FindPre,
			ParseEuler
		}

		/// <summary>
		/// 
		/// </summary>
		public int dataReceived { get; set; }

		/// <summary>
		/// 
		/// </summary>
		private bool ApplyOffsets = true;

		/// <summary>
		/// 
		/// </summary>
		/// <param name="comPort">Com port. "COM1"</param>
		/// <param name="parseAnalogs">true, if the analogdata of the gyros should also be parsed</param>
		public RazorAhrs(string comPort, bool parseAnalogs)
		{
			ParseAnalogs = parseAnalogs;
			serialHandle = new SerialPort(comPort, 57600, Parity.None, 8, StopBits.One);
			Initialize();
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="com"></param>
		/// <param name="parseAnalogs"></param>
		/// <param name="dtr"></param>
		public RazorAhrs(string com, bool parseAnalogs, Cpu.Pin dtr) : this(com, parseAnalogs)
		{
			dtr = dtr;
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="reset"></param>
		private void Initialize(bool reset = true)
		{
			serialHandle.ReadTimeout = 0;
			
			if (reset)
				Reset();

			// If you open the port after you set the event you will endup with problems
			serialHandle.Open();
			serialHandle.DiscardInBuffer();

			// Subscribe to RX events
			serialHandle.DataReceived += PortDataReceived;
			serialHandle.ErrorReceived += PortErrorReceived;
		}

		/// <summary>
		/// Reset the 9DoF
		/// </summary>
		private void Reset()
		{
			OutputPort dtr = new OutputPort(dtr, false);
			System.Threading.Thread.Sleep(1);

			dtr.Write(true);
			dtr.Dispose();
		}

		/// <summary>
		/// Parse recived data
		/// </summary>
		/// <param name="sender"></param>
		/// <param name="e"></param>
		void PortDataReceived(object sender, SerialDataReceivedEventArgs e)
		{
			int bufferIndex = 0;
			byte offsetIndex = 0;
			int dataReceived;

			dataReceived = serialHandle.Read(receiveBuffer, 0, receiveBuffer.Length);

			while (bufferIndex < dataReceived)
			{
				switch (imuState)
				{
					// Looking for IMU startup
					case ParserState.Start:
						if (this.receiveBuffer[bufferIndex] == productSring[preeIndex])
						{
							preeIndex++;

							if (this.preeIndex >= productSring.Length)
							{
								imuState = ParserState.FindOffsets;
								preeIndex = 0;
							}
						}

						break;

					// 
					case ParserState.FindOffsets:
						receivedChars[receiveIndex] = (char)receiveBuffer[bufferIndex];

						if (this.receivedChars[receiveIndex] == '\r' || receivedChars[receiveIndex] == '\n')
						{
							// Full line

							// More than \r\n ?
							if (this.receiveIndex > 2)
							{
								Debug.Print("X: " + receivedChars[receiveIndex]);
								Array.Copy(receivedChars, parseBuffer, receiveIndex);

								ParseOffset(out Offsets[offsetIndex++], parseBuffer);
								receiveIndex = 0;

								// Received all 6 offsets
								if (offsetIndex >= 6)
									imuState = ParserState.FindPre;
							}
						}
						else
							receiveIndex++;

						break;

					//
					case ParserState.FindPre:
						if (receiveBuffer[bufferIndex] == pree[preeIndex])
						{
							preeIndex++;
							if (preeIndex >= pree.Length)
							{
								imuState = ParserState.ParseEuler;
								preeIndex = 0;
							}
						}
						else
							preeIndex = 0;

						break;

					//
					case ParserState.ParseEuler:
						receivedChars[receiveIndex] = (char)receiveBuffer[bufferIndex];

						if (receivedChars[receiveIndex] == '\r' || receivedChars[receiveIndex] == '\n')
						{
							// Full Line
							if (receiveIndex > 10)
							{
								// Parse the Data!
								lock (parseBuffer)
								{
									Array.Copy(receivedChars, parseBuffer, receiveIndex);
									HasValidData = true;
								}

								haveToParse = true;
							}

							receiveIndex = 0;
							imuState = ParserState.FindPre;

						}
						else
						{
							receiveIndex++;

							if (receiveIndex >= receivedChars.Length)
							{
								Debug.Print("Sentence is too long!");
								receiveIndex = 0;
								imuState = ParserState.FindPre;
							}
						}

						break;
				}

				bufferIndex++;
			}
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="sender"></param>
		/// <param name="e"></param>
		private static void PortErrorReceived(object sender, SerialErrorReceivedEventArgs e)
		{
			Debug.Print("COM Error: " + e.EventType);
		}

		private static void ParseOffset(out int offsetVar, char[] data)
		{
			int i = 0;
			offsetVar = FastNumberParse.ParseInt(data, ref i);
		}

		/// <summary>
		/// Parses the received data
		/// Call it just before you use the data
		/// </summary>
		public void ParseData()
		{
			if (!haveToParse)
				return;

			lock (parseBuffer)
			{
				haveToParse = false;
				int i = 0;
				Roll = FastNumberParse.ParseFloat(parseBuffer, ref i);
				i++;
				Pitch = FastNumberParse.ParseFloat(parseBuffer, ref i);
				i++;
				Yaw = FastNumberParse.ParseFloat(parseBuffer, ref i);

				if (ParseAnalogs)
				{
					// + ',AN:'
					i += analogsPree.Length;

					if (ApplyOffsets)
					{
						AnX = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[0];
						i++;
						AnY = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[1];
						i++;
						AnZ = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[2];
						i++;
						AccX = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[3];
						i++;
						AccY = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[4];
						i++;
						AccZ = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[5];
					}
					else
					{
						AnX = FastNumberParse.ParseInt(parseBuffer, ref i);
						i++;
						AnY = FastNumberParse.ParseInt(parseBuffer, ref i);
						i++;
						AnZ = FastNumberParse.ParseInt(parseBuffer, ref i);
						i++;
						AccX = FastNumberParse.ParseInt(parseBuffer, ref i);
						i++;
						AccY = FastNumberParse.ParseInt(parseBuffer, ref i);
						i++;
						AccZ = FastNumberParse.ParseInt(parseBuffer, ref i);
					}

					i++;
					MagX = FastNumberParse.ParseInt(parseBuffer, ref i);
					i++;
					MagY = FastNumberParse.ParseInt(parseBuffer, ref i);
					i++;
					MagZ = FastNumberParse.ParseInt(parseBuffer, ref i);
				}
			}
		}

		/// <summary>
		/// Get compensated attitude output
		/// </summary>
		/// <param name="roll"></param>
		/// <param name="pitch"></param>
		/// <param name="yaw"></param>
		void IAhrs.GetAttitude(out double roll, out double pitch, out double yaw)
		{
			ParseData();
			roll = Roll;
			pitch = Pitch;
			yaw = Yaw;
		}

		/// <summary>
		/// Get analog output
		/// </summary>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="z"></param>
		void IAhrs.GetAnalogs(out double x, out double y, out double z)
		{
			ParseData();
			x = AnX;
			y = AnY;
			z = AnZ;
		}

		/// <summary>
		/// Get accelerometer output
		/// </summary>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="z"></param>
		void IAhrs.GetAcc(out double x, out double y, out double z)
		{
			ParseData();
			x = AccX;
			y = AccY;
			z = AccZ;
		}
	}
}